﻿Imports System
Imports System.IO.Ports
Imports System.Net
Imports System.Net.Sockets
Imports System.Collections.Concurrent
Imports System.Collections.Generic
Imports System.Threading
Imports System.Threading.Tasks
Imports Alchemy
Imports Alchemy.Classes
Imports Newtonsoft.Json
Imports SlimDX
Imports SlimDX.XInput

Public Class Robotics_DirectControl

    Private Declare Sub Sleep Lib "kernel32" (ByVal dwMilliseconds As Integer) 'Can sleep if needed
    ' Initialize Com Port
    Private Sub Robotics_Control_Initialize(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles MyBase.Load
        CommPortSetup() ' Calls function to establish the serial COM port
        Validate() ' Calls function to validate data that will be sent
        Timer1.Interval() = 50 'Sets the interval for the Timer to be in ms
        Timer1.Start() ' This starts the timer 

    End Sub

    '********** Drive State ***********
    Dim actuatorState = "stopped"
    Dim brushState = "stopped"
    Dim driveState = "stopped"
    Dim BactuatorState = "stopped"

    '********** Server Variables **********
    Dim clients As New ConcurrentDictionary(Of UserContext, Integer)() ' List of Connected WebSocket Clients <UserContext, Client Number>
    Dim wsServer                                                       ' Stores an instance of the Server
    Dim quittingTime = False                                           ' Time to end test message
    Delegate Sub UpdateConsoleCallback(ByVal [s] As String)            ' Delegate to update UI

    Dim port = 5001 ' WebSocket Server Port
    Private mySerialPort As New SerialPort
    ' Data Structure to Store Message from Client
    Public Class Message
        Public msg As String
    End Class
    '********** Drive Variables **********
    Dim address As Byte
    Dim command As Byte
    Dim data As Byte
    Dim checksum As Byte
    Dim controller1 As Byte = 128 ' The Address of Controller 1 (drive train Mixed MODE)
    Dim controller2 As Byte = 129 ' The Address of Controller 2 (Big actuator,Brush) (not mixed)
    Dim controller3 As Byte = 130 ' The Address of Controller 3 (Back Actuators(as one devices In sync) (not mixed mode))
    Dim forward_mixed As Byte = 8 ' Command byte to drive forward (mixed mode)
    Dim back_mixed As Byte = 9 ' Command byte to drive backwards (mixed mode)
    Dim right_mixed As Byte = 10 ' Command byte to turn right (mixed mode)
    Dim left_mixed As Byte = 11 ' Command byte to turn left (mixed mode)
    Dim actuator_up As Byte = 0 ' Command byte to make the actuator go up
    Dim actuator_down As Byte = 1 ' Command byte to make the actuator go down
    Dim brush_start As Byte = 5 ' Command byte to make the brush start
    Dim power As Integer ' Used for variable brush speed
    Private Controllers As Controller() = New Controller(UserIndex.One) {}
    Public Sub New()
        InitializeComponent()
        Controllers(0) = New Controller(UserIndex.One)
    End Sub
    ' This function will be called at form load to set up the COM port using 8N1 protocol (8N1/9600)
    Private Sub CommPortSetup()
        With mySerialPort
            .PortName = "COM3"
            .BaudRate = 9600
            .DataBits = 8
            .Parity = Parity.None
            .StopBits = StopBits.One
            .Handshake = Handshake.None
        End With
        Try
            mySerialPort.Open()
        Catch ex As Exception
            MessageBox.Show(ex.Message)
        End Try
    End Sub
    ' Sabertooth control code Packetized Serial
    Public Sub Drive(ByVal address, ByVal command, ByVal data)
        checksum = ((address + command + data) And 127) ' Calculating the checksum 

        Dim TxBuffer() As Byte = {address, command, data, checksum} ' Packing the data into a packet for serial transit
        mySerialPort.Write(TxBuffer, 0, TxBuffer.Length) ' Sends the data on the serial port
        'Testing the Data Sent is Correct
        'MessageBox.Show(address)
        'MessageBox.Show(command)
        'MessageBox.Show(data)
        'MessageBox.Show(checksum)

    End Sub
    ' Before Acting on any Mixed Mode Data (used for differential steering on controller one) the controller must have valid data for Drive and Turn before using any commands
    ' This code sends 0 (stop) to  forward,reverse,left,right. After this data is recieved it can be updated with the drive and turn commands.
    Public Sub Validate()
        Drive(controller1, forward_mixed, 0)
        Drive(controller1, back_mixed, 0)
        Drive(controller1, right_mixed, 0)
        Drive(controller1, left_mixed, 0)
    End Sub

    '**********************************************
    '*************** Click Handlers ***************
    '**********************************************

    ' BtnBrush 0% Power Click Handler
    Private Sub BtnBrush_power0_Click(sender As Object, e As EventArgs) Handles BtnBrush_power0.Click

    End Sub

    ' BtnBrush 25% Power Click Handler
    Private Sub BtnBrush_power25_Click(sender As Object, e As EventArgs) Handles BtnBrush_power25.Click
        BrushPower(25)
    End Sub

    ' BtnBrush 50% Power Click Handler
    Private Sub BtnBrush_power50_Click(sender As Object, e As EventArgs) Handles BtnBrush_power50.Click
        BrushPower(50)
    End Sub

    ' BtnBrush 75% Power Click Handler
    Private Sub BtnBrush_power75_Click(sender As Object, e As EventArgs) Handles BtnBrush_power75.Click
        BrushPower(75)
    End Sub

    ' BtnBrush 100% Power Click Handler
    Private Sub BtnBrush_power100_Click(sender As Object, e As EventArgs) Handles BtnBrush_power100.Click
        BrushPower(100)
    End Sub

    ' BtnForward Click Handler
    Private Sub BtnForward_Drivetrain_Click(ByVal sender As System.Object, ByVal e As System.EventArgs) Handles BtnForward_Drivetrain.Click
        Forward()
    End Sub

    ' BtnBackward Click Handler
    Private Sub BtnBackward_Drivetrain_Click(sender As Object, e As EventArgs) Handles BtnBackward_Drivetrain.Click
        Backward()
    End Sub

    ' BtnLeft Click Handler
    Private Sub BtnLeft_Drivetrain_Click(sender As Object, e As EventArgs) Handles BtnLeft_Drivetrain.Click
        GoLeft()
    End Sub

    ' BtnRight Click Handler
    Private Sub BtnRight_Drivetrain_Click(sender As Object, e As EventArgs) Handles BtnRight_Drivetrain.Click
        GoRight()
    End Sub

    ' BtnStop Click Handler
    Private Sub BtnStop_Drivetrain_Click(sender As Object, e As EventArgs) Handles BtnStop_Drivetrain.Click
        StopMoving()
    End Sub

    ' BtnUp Click Handler
    Private Sub BtnUp_Actuator_Click(sender As Object, e As EventArgs) Handles BtnUp_Actuator.Click
        ActuatorUp()
    End Sub

    ' BtnDown Click Handler
    Private Sub BtnDown_Actuator_Click(sender As Object, e As EventArgs) Handles BtnDown_Actuator.Click
        ActuatorDown()
    End Sub

    ' BtnStop Click Handler
    Private Sub BtnStop_Actuator_Click(sender As Object, e As EventArgs) Handles BtnStop_Actuator.Click
        ActuatorStop()
    End Sub
    '********************************************
    '*************** Control Subs ***************
    '********************************************

    ' Set Brush Power
    Public Sub BrushPower(ByVal power As Integer)
        Select Case (power)
            Case 0 To 50
                If Not brushState = "stopped" Then
                    Drive(controller2, brush_start, 0)
                    display("Brush Stopped")
                    brushState = "stopped"
                End If
            Case 51 To 101
                If Not brushState = "25" Then
                    Drive(controller2, brush_start, 31)
                    display("Brush at 25%")
                    brushState = "25"
                End If
            Case 102 To 152
                If Not brushState = "50" Then
                    Drive(controller2, brush_start, 62)
                    display("Brush at 50%")
                    brushState = "50"
                End If
            Case 153 To 203
                If Not brushState = "75" Then
                    Drive(controller2, brush_start, 93)
                    display("Brush at 75%")
                    brushState = "75"
                End If
            Case 204 To 255
                If Not brushState = "100" Then
                    Drive(controller2, brush_start, 127)
                    display("Brush at Full")
                    brushState = "100"
                End If
        End Select

    End Sub

    ' Go Forwards
    Public Sub Forward()
        If Not driveState = "forward" Then
            display("Going Forward")
            Drive(controller1, forward_mixed, 127)
            driveState = "forward"
        End If
    End Sub

    ' Go Backwards
    Public Sub Backward()
        If Not driveState = "backward" Then
            display("Going Backward")
            Drive(controller1, back_mixed, 127)
            driveState = "backward"
        End If
    End Sub

    ' Go Left
    Public Sub GoLeft()
        If Not driveState = "left" Then
            display("Going Left")
            'Drive(controller1, forward_mixed, 127) ' Since turning is a MIXED MODE command it must have valid data to differentiate the wheels. This sets speed to full forward before the turn commmand is issued.
            Drive(controller1, left_mixed, 127)
            driveState = "left"
        End If
    End Sub

    ' Go Right
    Public Sub GoRight()
        If Not driveState = "right" Then
            display("Going Right")
            'Drive(controller1, forward_mixed, 127) ' Since turning is a MIXED MODE command it must have valid data to differentiate the wheels. This sets speed to full forward before the turn commmand is issued.
            Drive(controller1, right_mixed, 127)
            driveState = "right"
        End If
    End Sub

    ' Stop
    Public Sub StopMoving()
        If Not driveState = "stopped" Then
            display("Stopping")
            Drive(controller1, forward_mixed, 0)
            Drive(controller1, back_mixed, 0)
            Drive(controller1, left_mixed, 0)
            Drive(controller1, right_mixed, 0)
            driveState = "stopped"
        End If
    End Sub

    ' Actuator Up
    Public Sub ActuatorUp()
        If Not actuatorState = "up" Then
            display("Actuator Going Up")
            Drive(controller2, actuator_up, 127)
            actuatorState = "up"
        End If
    End Sub

    ' Actuator Down
    Public Sub ActuatorDown()
        If Not actuatorState = "down" Then
            display("Actuator Going Down")
            Drive(controller2, actuator_down, 127)
            actuatorState = "down"
        End If
    End Sub

    ' Actuator Stop
    Public Sub ActuatorStop()
        If Not actuatorState = "stopped" Then
            display("Actuator Stopping")
            Drive(controller2, actuator_up, 0) ' Since actuator_up/actuator_down are only commands bits that signify the direction you can use either to stop by sending a zero
            actuatorState = "stopped"
        End If
    End Sub
    'Back Actuator Up
    Public Sub BActuatorUP()
        If Not BactuatorState = "up" Then
            display("Back Actuator Going Up")
            Drive(controller3, actuator_up, 127)
            BactuatorState = "up"
        End If
    End Sub
    'Back Actuator Down
    Public Sub BActuatorDown()
        If Not BactuatorState = "down" Then
            display("Back Actuator Going Down")
            Drive(controller3, actuator_down, 127)
            BactuatorState = "down"
        End If
    End Sub
    'Back Actuator Stop
    Public Sub BActuatorStop()
        If Not BactuatorState = "stopped" Then
            display("Back Actuator Stopping")
            Drive(controller3, actuator_up, 0)
            BactuatorState = "stopped"
        End If
    End Sub


     

    '********************************************
    '****************** Server ******************
    '********************************************

    'Update Console Display - DO NOT CALL
    Sub updateConsole(ByVal [s] As String)
        Me.consoleBox.AppendText(DateTime.Now.TimeOfDay.ToString + " " + s + vbCrLf)
    End Sub

    'Decide Method and Update Console Display - Thread Safe
    Private Sub display(ByVal [s] As String)
        If Me.consoleBox.InvokeRequired Then
            Dim d As New UpdateConsoleCallback(AddressOf updateConsole)
            Me.Invoke(d, New Object() {[s]})
        Else
            updateConsole([s])
        End If
    End Sub

    'Start Server Button
    Private Sub btnStartServer_Click(sender As Object, e As EventArgs) Handles btnStartServer.Click
        btnEndServer.Enabled = True
        btnStartServer.Enabled = False
        start()
    End Sub

    'Stop Server Button
    Private Sub btnEndServer_Click(sender As Object, e As EventArgs) Handles btnEndServer.Click
        btnEndServer.Enabled = False
        btnStartServer.Enabled = True
        quit()
    End Sub

    'Server
    Private Sub start()
        consoleBox.AppendText("Middle Tennessee State University" + vbCrLf)
        consoleBox.AppendText("RaiderRobotics 2013 - NASA Lunabotics Competition" + vbCrLf)
        consoleBox.AppendText(vbCrLf)
        consoleBox.AppendText("WebSocket Server:" + vbCrLf)
        consoleBox.AppendText("-----------------------------------------------------------" + vbCrLf)

        'Start WebSocket Server
        display("Starting WebSocket Server")

        'Creates server and binds events
        wsServer = New Alchemy.WebSocketServer(port, IPAddress.Any) With { _
            .OnReceive = AddressOf OnReceive, _
            .OnSend = AddressOf OnSend, _
            .OnConnected = AddressOf OnConnect, _
            .OnDisconnect = AddressOf OnDisconnect, _
            .TimeOut = New TimeSpan(0, 5, 0) _
        }

        wsServer.Start()

        display("Server started")
    End Sub

    'Handles a Client Connecting
    Public Sub OnConnect(context As UserContext)
        display(context.ClientAddress.ToString + " just connected.")
        clients.TryAdd(context, clients.Count) 'Add client to client dictionary
    End Sub

    'Handles a Client Disconnecting
    Public Sub OnDisconnect(context As UserContext)
        Dim returnVal As Integer              'Stores value returned by TryRemove.  We don't need the value.
        display(context.ClientAddress.ToString + " just disconnected.")
        clients.TryRemove(context, returnVal) 'Remove client from client dictionary
    End Sub

    'Handles Message Reciept
    'TODO - Rewrite this to handle JSON messages from Client
    Public Sub OnReceive(context As UserContext)
        display("Received message from " + context.ClientAddress.ToString)

        Try
            Dim msg = context.DataFrame.ToString

            ' Select Appropriate Action Based on Message from Client
            Select Case (msg)
                Case "GoForward"
                    Forward()
                Case "GoBackward"
                    Backward()
                Case "GoLeft"
                    GoLeft()
                Case "GoRight"
                    GoRight()
                Case "Stop"
                    StopMoving()
                Case "ActuatorUp"
                    ActuatorUp()
                Case "ActuatorDown"
                    ActuatorDown()
                Case "ActuatorStop"
                    ActuatorStop()
                Case "BrushAt0"
                    BrushPower(0)
                Case "BrushAt25"
                    BrushPower(51)
                Case "BrushAt50"
                    BrushPower(102)
                Case "BrushAt75"
                    BrushPower(153)
                Case "BrushAt100"
                    BrushPower(204)
            End Select

        Catch e As Exception
            display("Bad JSON Message")
        End Try
    End Sub

    'Notification that a Message was Sent
    Public Sub OnSend(context As UserContext)
        display("Sent message to " + context.ClientAddress.ToString)
    End Sub

    'Quit 
    Public Sub quit()
        quittingTime = True
        wsServer.Stop()
    End Sub

    Private Sub Timer1_Tick(sender As Object, e As EventArgs) Handles Timer1.Tick
        ' Xbox 360 Controller Code 
        'Safetey Feature - If the controller is not connected or becomes disconnected while in use all motors are turned off

        ' Dim prevstate ' Needed to be added for latency 
        If Controllers(0).IsConnected Then
            Dim currentstate = Controllers(0).GetState.Gamepad.Buttons

            ' Manage the Actuators
            If (currentstate And GamepadButtonFlags.LeftShoulder) <> 0 Then
                ActuatorDown()
            ElseIf (currentstate And GamepadButtonFlags.RightShoulder) <> 0 Then
                ActuatorUp()
            ElseIf (currentstate And GamepadButtonFlags.X) <> 0 Then
                BActuatorUP()
            ElseIf (currentstate And GamepadButtonFlags.A) <> 0 Then
                BActuatorDown()
            Else
                ActuatorStop()
                BActuatorStop()
            End If

            ' Manage the Drive
            If (currentstate And GamepadButtonFlags.DPadUp) <> 0 Then
                Forward()
            ElseIf (currentstate And GamepadButtonFlags.DPadDown) <> 0 Then
                Backward()
            ElseIf (currentstate And GamepadButtonFlags.DPadLeft) <> 0 Then
                GoLeft()
            ElseIf (currentstate And GamepadButtonFlags.DPadRight) <> 0 Then
                GoRight()
            Else
                StopMoving()
            End If

            ' Manage the Brush
            BrushPower((Controllers(0).GetState.Gamepad.RightTrigger))
        End If
        ' prevstate = currentstate
    End Sub

    Private Sub PictureBox1_Click(sender As Object, e As EventArgs) Handles PictureBox1.Click

    End Sub
End Class
